#include "operate.h"
#include "Q_to_Q.h"
#include "IMU_Task.h"
#include "arm_math.h"

#define DATA_D {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1}

#define D_L_AB 20.0f
#define D_L_BC 25.0f
#define D_L_CD 5.0f

float q_local[4] = {1,0,0,0};//大地四元数
float q_arm[4] = {1,0,0,0};//大臂四元数
float q_fing[4] = {1,0,0,0};

T_Matrix_Data_typdef T_Matrix_Data[3];//变换矩阵数据源
float S_out_f[2][16] = {DATA_D,DATA_D};//运算过程中的中间矩阵数据源

Ax6_t Ax6;

arm_matrix_instance_f32 S_in[3];//变换矩阵（T矩阵）
arm_matrix_instance_f32 S_out[2];//运算过程中的中间矩阵
arm_matrix_instance_f32 OUT;//运算输出矩阵
arm_matrix_instance_f32 IN;//运算输入矩阵

void Matrixs_Init(void)
{
	/*三个T矩阵初始化*/
    arm_mat_init_f32(&S_in[0],4,4,T_Matrix_Data[0].T_out);
    arm_mat_init_f32(&S_in[1],4,4,T_Matrix_Data[1].T_out);
    arm_mat_init_f32(&S_in[2],4,4,T_Matrix_Data[2].T_out);

    /*两个中间矩阵初始化*/
    arm_mat_init_f32(&S_out[0],4,4,S_out_f[0]);
    arm_mat_init_f32(&S_out[1],4,4,S_out_f[1]);
}

void Quaternion_to_T_Matrix(float *Q_dest , float *Q_src , T_Matrix_Data_typdef *T_Matrix , float L)
{
	float q_inv[4];
	float q_out[4];
	
	quaternion_inverse(Q_src , q_inv);
	quaternion_multiply(Q_dest , q_inv , q_out);

    T_Matrix->T_out[0] = 1 - 2*q_out[2]*q_out[2] - 2*q_out[3]*q_out[3];
    T_Matrix->T_out[1] = 2*q_out[1]*q_out[2] - 2*q_out[3]*q_out[0];
    T_Matrix->T_out[2] = 2*q_out[1]*q_out[3] + 2*q_out[2]*q_out[0];

    T_Matrix->T_out[4] = 2*q_out[1]*q_out[2] + 2*q_out[3]*q_out[0];
    T_Matrix->T_out[5] = 1 - 2*q_out[1]*q_out[1] - 2*q_out[3]*q_out[3];
    T_Matrix->T_out[6] = 2*q_out[2]*q_out[3] - 2*q_out[1]*q_out[0];

    T_Matrix->T_out[8] = 2*q_out[1]*q_out[3] - 2*q_out[2]*q_out[0];
    T_Matrix->T_out[9] = 2*q_out[2]*q_out[3] + 2*q_out[1]*q_out[0];
    T_Matrix->T_out[10] = 1 - 2*q_out[1]*q_out[1] - 2*q_out[2]*q_out[2];
	
	T_Matrix->T_out[3] = L * T_Matrix->T_out[0];
	T_Matrix->T_out[7] = L * T_Matrix->T_out[4];
	T_Matrix->T_out[11] = L * T_Matrix->T_out[8];
	
	T_Matrix->T_out[12] = 0;
	T_Matrix->T_out[13] = 0;
	T_Matrix->T_out[14] = 0;
	T_Matrix->T_out[15] = 1;
}

void Operate_Task(void)
{
	/*三个T矩阵源数组数据更新*/
	Quaternion_to_T_Matrix(q_arm, q_local, &T_Matrix_Data[0], D_L_AB);
	Quaternion_to_T_Matrix(QEKF_INS.q, q_arm, &T_Matrix_Data[1], D_L_BC);
	Quaternion_to_T_Matrix(q_fing, QEKF_INS.q, &T_Matrix_Data[2], D_L_CD);

	/*三个T矩阵相乘*/
    arm_mat_mult_f32(&S_in[0],&S_in[1],&S_out[0]);
    arm_mat_mult_f32(&S_out[0],&S_in[2],&S_out[1]);
	
	Ax6.Ax6_P.P_X = S_out_f[1][3];
	Ax6.Ax6_P.P_Y = S_out_f[1][7];
	Ax6.Ax6_P.P_Z = S_out_f[1][11];
	
	//BMI088
//	Ax6.Ax6_R.rpy_P = QEKF_INS2.Pitch;
//	Ax6.Ax6_R.rpy_R = QEKF_INS2.Roll;
//	Ax6.Ax6_R.rpy_Y = QEKF_INS2.Yaw;
	
	//ICM20602
	Ax6.Ax6_R.rpy_P = asinf(q_fing[1])*113.6f;
	Ax6.Ax6_R.rpy_R = asinf(q_fing[2])*113.6f;
	Ax6.Ax6_R.rpy_Y = asinf(q_fing[3])*113.6f;
}

